#!/usr/bin/env python

import roslib; roslib.load_manifest('brain_stuff')
import rospy
import image_geometry
import message_filters
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge, CvBridgeError
import cv
import math
import copy
import sys
import array
from PySide.QtCore import *
from PySide.QtGui import *
import smach
import threading

# Convert cv image to qimage...
class IplQImage(QImage):
   def __init__(self, iplimage):
      rgba = cv.CreateMat(iplimage.height, iplimage.width, cv.CV_8UC4)
      cv.Set(rgba, (1,2,3,4))

      cv.MixChannels([iplimage], [rgba], [ (0,0), (1,1), (2,2), (3,3) ] ) # This encoding works because I explicitly convert the imgmsg to cv in rgba
      self.__imagedata = rgba.tostring()
      super(IplQImage,self).__init__(self.__imagedata, iplimage.width, iplimage.height, QImage.Format_RGB32)

class QSignal(QObject):
   signal = Signal()

class Interface(smach.State):
   def __init__(self):
      # Smach
      smach.State.__init__(self, outcomes=['success', 'aborted'])
      # Flags and global vars
      self.qimage = QImage()
      # OpenCv setup
      self.bridge = CvBridge()
      # Publishers and Subscribers
      self.image_sub = rospy.Subscriber("/wide_stereo/right/image_rect_color", Image, self.callback)
      rospy.loginfo("Waiting for transform from base_link to wide_stereo_optical_frame")
      # Pyside
      self.imageReady = QSignal()
      self.imageLock = threading.Lock()
      self.group = QGroupBox('group1')
      self.label2 = QLabel()
      vbox = QVBoxLayout()
      vbox.addWidget(self.label2)
      self.group.setLayout(vbox)

   def execute(self, userdata):
      try:
         #print type(self.imageReady)
         #import pprint
         #pprint.pprint(dir(self.imageReady))
         #return 'aborted'
         self.imageReady.signal.connect(self.showImage)
         self.group.show()
         app.exec_()
      except KeyboardInterrupt:
         app.quit()
         return 'aborted'
      app.quit()
      return 'success' 

   def showImage(self):
      rospy.loginfo("showImage")

      self.imageLock.acquire()
      qpixmap = QPixmap.fromImage(self.qimage)
      self.label2.setPixmap(qpixmap)
      self.imageLock.release()

      
   def callback(self, data):
      rospy.loginfo("Received image")
      if rospy.is_shutdown():
         return
      try:
         cv_image = self.bridge.imgmsg_to_cv(data, "rgba8")
      except CvBridgeError, e:
         print e
         return
      self.imageLock.acquire()
      self.qimage = IplQImage(cv_image)
      self.imageLock.release()
      self.imageReady.signal.emit()

if __name__== "__main__":
   rospy.init_node('test_interface')

   app = QApplication(sys.argv)
   machine = smach.StateMachine(outcomes = ['success'])
   with machine:
      smach.StateMachine.add("Interface", Interface(), transitions={'aborted':'success', 'success':'Interface'})
   try:
      machine.execute()
   except KeyboardInterrupt:
      exit()


